#include <examples.h>
#ifdef MOTOR
#include <device/dji_motor.h>
#include <device/damiao_motor.h>
#include <sheriffos.h>

DjiMotor<Dji6020V, 0x205> motor1(1, 0_deg);
DjiMotor<Dji6020V, 0x206> motor2(1, 0_deg);

auto motor_group1 = make_DjiMotorGroup<os::Can<1>, 0x1ff>(motor1);
auto motor_group2 = make_DjiMotorGroup<os::Can<3>, 0x1ff>(motor2);

InitLate(aa) {
    os::Can<1>::instance().onReceive_.connect([](FDCAN_RxHeaderTypeDef* header, uint8_t* buffer) {
        motor_group1.decode(header->Identifier, buffer);
    });
    os::Can<3>::instance().onReceive_.connect([](FDCAN_RxHeaderTypeDef* header, uint8_t* buffer) {
        motor_group2.decode(header->Identifier, buffer);
    });
}

CREATE_THREAD_STATIC(shtt, 512, NULL, 5) {
    __cplusplus;

    for (;;) {
        motor1.setOutput(5000);
        motor2.setOutput(10000);
        motor_group1.transmitData();
        motor_group2.transmitData();
        os::sleep(1_ms);
    }
}
#endif